#include "mm.h"
#include "sysregs.h"
#include "asm/mmpage_pro.h"
//#include "asm/mmpage_hwdef.h"

.section .rodata
.align 3
.globl el_string1
el_string1:
	.string "Booting at EL"

.section ".text.boot"
.globl _start
_start:
	mrs	x0, mpidr_el1		
	and	x0, x0,#0xFF		// 检查处理器核心ID
	cbz	x0, master		// 除了CPU0，其他CPU都会在这里死循环等待
	b	proc_hang

proc_hang: 
	b 	proc_hang

master:
	bl __init_uart

	mrs x5, CurrentEL
	cmp x5, #CurrentEL_EL3
	b.eq el3_entry
	b el2_entry

el3_entry:
	eret


#ifdef CONFIG_VIRT
el2_entry:
	bl print_el
	
	ldr x0, =(3UL << 20);
	msr CPTR_EL2, x0

	ldr x5, =virt_vectors
	msr vbar_el2, x5
	isb

	b boot_kernel

#else
el2_entry:
	bl print_el

	ldr x0, =HCR_HOST_NVHE_FLAGS
	msr hcr_el2, x0
	ldr x0, =SCTLR_VALUE_MMU_DISABLED
	msr sctlr_el1, x0
	ldr x0, =SPSR_EL1
	msr spsr_el2, x0
	adr x0, el1_entry //el1 entry addr 
	msr elr_el2, x0

	eret
#endif

el1_entry:
	bl print_el
/*
	adr x0, TEXT_ROM
	adr x1, _text
	adr x2, _etext
1:
	ldr x4, [x0], #8
	str x4, [x1], #8
	cmp x1, x2
	b.cc 1b
	*/

	ldr x5, =vectors
	msr vbar_el1, x5
	isb

boot_kernel:
	adr	x0, bss_begin
	adr	x1, bss_end
	sub	x1, x1, x0
	bl 	memzero

	bl create_page_table

#ifdef CONFIG_VIRT
	bl enable_mmu_el2
#else
	bl enable_mmu_el1
#endif

	//adr x2, kernel_sp
	//add  sp, x2, #4096	
	
    mov sp, #0x100000
    
    // 测试简单函数调用
    //bl test_function
	bl	init_main
    b proc_hang



print_el:
	mov x10, x30

	adrp x0, el_string1
	add x0, x0, :lo12:el_string1
	bl put_string_uart

	mrs x5, CurrentEL
	lsr x2, x5, #2
	mov x0, #48
	add x0, x0, x2

	bl put_uart
	mov x0, #10
	bl put_uart

	mov x30, x10
	ret


/*    High    */
/* init_pg_dir */
/* bss       */
/* data(idmap_pg_dir 3M)   */
/* _rodata   */
/* text      */
/* text.boot */
/* 0x80000   */
/*    Low     */

create_page_table:
	/*Save Reg LR*/
	mov x25, x30

	/*clean mmpage*/
	adrp x0, idmap_pg_dir
	adrp x1, idmap_pg_end
	sub x1, x1, x0 //x1 = pagesize
	bl memzero

	adrp x1, _text_boot
	and x1, x1, SECTION_MASK //why section
	mov x2, x1
	adrp x3, _end
	add x3, x3, #SECTION_SIZE
	sub x3, x3, #1
	and x3, x3, SECTION_MASK
	mov x4, SWAPPER_MM_MMUFLAGS
	adrp x0, idmap_pg_dir
	
	bl __create_section_mapping

	mov x30, x25
	ret


/*set mair reg + TCR reg +sctlr reg + idmap_pg_dir to ttbr*/
enable_mmu_el1:
	tlbi	vmalle1	// Invalidate local TLB
	dsb nsh
	/*config mair reg*/
	ldr	x5, =MAIR(0x00, MT_DEVICE_nGnRnE) | \
		     MAIR(0x04, MT_DEVICE_nGnRE) | \
		     MAIR(0x0c, MT_DEVICE_GRE) | \
		     MAIR(0x44, MT_NORMAL_NC) | \
		     MAIR(0xff, MT_NORMAL) | \
		     MAIR(0xbb, MT_NORMAL_WT)
	msr mair_el1, x5

	ldr	x10, =TCR_TxSZ(VA_BITS) | TCR_TG_FLAGS | TCR_CACHE_FLAGS
	msr tcr_el1, x10

	ldr x3, =(SCTLR_ELx_M | SCTLR_ELx_C)

	adrp x0, idmap_pg_dir
	msr ttbr0_el1, x0
	isb
	msr sctlr_el1, x3
	isb

	ic iallu
	dsb nsh
	isb

	ret

/*el2 config*/
enable_mmu_el2:
	tlbi alle2
	dsb nsh
	/*config mair reg*/
	ldr	x5, =MAIR(0x00, MT_DEVICE_nGnRnE) | \
		     MAIR(0x04, MT_DEVICE_nGnRE) | \
		     MAIR(0x0c, MT_DEVICE_GRE) | \
		     MAIR(0x44, MT_NORMAL_NC) | \
		     MAIR(0xff, MT_NORMAL) | \
		     MAIR(0xbb, MT_NORMAL_WT)
	msr mair_el2, x5

	ldr	x10, =TCR_TxSZ(VA_BITS) | TCR_TG_FLAGS | TCR_CACHE_FLAGS
	msr tcr_el2, x10

	ldr x3, =(SCTLR_ELx_M | SCTLR_ELx_C)

	adrp x0, idmap_pg_dir
	msr ttbr0_el2, x0
	isb
	msr sctlr_el2, x3
	isb

	ic iallu
	dsb nsh
	isb
	ret


/*
   x0: page table base address
   x1: phys address
   x2: virt start address
   x3: virt end address
   x4: prot
 */

 /* PGD */
 /*  */
__create_section_mapping:
	mov x12, x30
	/* x12 = x30*/

	/*get PGD var*/
	lsr x8, x2, #PGDIR_SHIFT
	and x8, x8, #PTRS_PER_PGD - 1

	/*fll pgd */
	add x9, x0, #PAGE_SIZE
	orr x9, x9, #PUD_TYPE_TABLE
	str x9, [x0, x8, lsl #3]

	add x0, x0, #PAGE_SIZE

	/*pud Var*/
	lsr x8, x2, #PUD_SHIFT
	and x8, x8, #PTRS_PER_PUD -1 

	/*fill pud */
	add x9, x0, #PAGE_SIZE
	orr x9, x9, #PUD_TYPE_TABLE
	str x9, [x0, x8, lsl #3]
	add x0, x0, #PAGE_SIZE

	/*get pmd var*/
1:
	/* block mm*/
	lsr x8, x2, #PMD_SHIFT
	and x8, x8, #PTRS_PER_PMD - 1
	lsr x10, x1, #SECTION_SHIFT
	mov x9, x4
	orr x10, x9, x10, lsl #SECTION_SHIFT
	str x10, [x0, x8, lsl #3]

	add x2, x2, #PMD_SIZE
	add x1, x1, #PMD_SIZE

	cmp x3, x2
	b.hi 1b
	
	mov x30, x12
	ret
	
